﻿
#pragma once

#include "../SerialPort/Protocol.h"
#include "../Config/FightingParam.h"
#include "opencv2/opencv.hpp"
#include<opencv2/ml/ml.hpp>

class ArmorDetector {
public:
    ArmorDetector();
    ~ArmorDetector();
    bool DetectArmor(cv::Mat& src, cv::Point3f& target);

private:
    int now_frame;
    std::vector<float > num_infantry;
    int direction;//不清楚0左1右2
	
    bool enable_debug;
    /*debug用*/
    cv::Mat show_lights; /*灯条帧*/
    cv::Mat show_armors; /*装甲板帧*/
    cv::Mat show_final_armor; /*最终结果帧*/

    cv::Mat DistillationColor(const cv::Mat& src);
    inline void DrawRotatedRect(const cv::Mat& image, const cv::RotatedRect& rect, const cv::Scalar& color, int thickness);
    void DetectLights(const cv::Mat& src, std::vector<cv::RotatedRect>& lights);

    inline void DrawArmor(const cv::Mat& image, const std::vector<cv::Point2f>& armor, const cv::Scalar& color, int thickness);
    void PossibleArmors(const std::vector<cv::RotatedRect>& lights, std::vector<std::vector<cv::Point2f>>& armors);
    void CalcArmorInfo(std::vector<cv::Point2f>& armor, const cv::RotatedRect& left_light, const cv::RotatedRect& right_light);

    const std::vector<cv::Point2f> armor_points { cv::Point2f(0, 0), cv::Point2f(armorParam.small_armor_width, 0),
        cv::Point2f(armorParam.small_armor_width, armorParam.armor_height), cv::Point2f(0, armorParam.armor_height) };
    std::vector<cv::Mat> number_template;
    int DetectNumber(const cv::Mat& perspective);
    /* 基地8(level_5) > 哨兵7(level_4) > 英雄1(level_3) > 步兵345(level_2) > 工程2(level_1) > 无0(level_0)) */
    const int number_level[9] = { 0, 3, 1, 2, 2, 2, 3, 4, 5 };
    void SelectFinalArmor(const cv::Mat& src, std::vector<std::vector<cv::Point2f>>& armors, std::vector<cv::Point2f>& final_armor,int &num);
    void CalcControlInfo(const std::vector<cv::Point2f>& armor, cv::Point3f& target);
    void CalsolvePnP(const std::vector<cv::Point2f>& armor, cv::Point3f& target);
    
    cv::RNG rng;
    cv::Ptr<cv::ml::SVM> svm;

    int stateNum = 4;                                      //状态值4×1向量(x,y,△x,△y)
    int measureNum = 2;
    cv::KalmanFilter KF;
    cv::Mat measurement;
    void Kalman_Predict(const std::vector<cv::Point2f>& armor, cv::Point3f& target);

};
